#include "utils.h"

Utils::Utils(QVector<Point> &points,double tInit,double tFinal,double alpha)
{
    this->tInit=tInit;
    this->tFinal=tFinal;
    this->alpha=alpha;
    this->maxMarkov=1000;
    this->beta=1.015;
    this->coordinates=nc::NdArray<Point>(points.begin(),points.end());
    this->nCities=points.size();
    this->nMarkov=this->nCities;
    this->tNow=tInit;
}

void Utils::runTSP()
{
    init();
    int iter=0;
    while(tNow>tFinal){
        for(int k=0;k<nMarkov;k++){
            auto turbRand=nc::random::rand<double>();
            auto tourNew=nc::NdArray<int>();
            double dE;
            if(turbRand<0.4){
                mutateInvE(tourNew,dE);
            }else if(turbRand<0.7){
                mutateMoveE(tourNew,dE);
            }else{
                mutateSwapE(tourNew,dE);
            }
            auto valueNew=calcTourMile(tourNew,nCities);
            auto deltaE=valueNew-valueNow;
            if(abs(deltaE-dE)>1e-3){
                throw "Error in deltaE verify!";
            }
            bool accept=false;
            if(deltaE<0){
                accept=true;
                if(valueNew<valueBest){
                    tourBest=tourNew;
                    valueBest=valueNew;
                }
            }
            else{
                auto pAccept=exp(-deltaE/tNow);
                if(pAccept>nc::random::rand<double>()) accept=true;
            }
            if(accept){
                tourNow=tourNew;
                valueNow=valueNew;
            }
        }
        tourNow=nc::roll(tourNow,2);
        recordBest.push_back(valueBest);
        recordNow.push_back(valueNow);

        iter++;
        tNow=tNow*alpha;
        nMarkov=std::min(round(nMarkov * beta), maxMarkov);
    }
}

QVector<int> Utils::getTourBest()
{
    QVector<int> res;
    for(int i:this->tourBest){
        res.push_back(i);
    }
    return res;
}

QString Utils::getTourBestStr()
{
    QString str;
    for(int i:this->tourBest){
        str+=QString::number(i)+"->";
    }
    str+=QString::number(tourBest[0]);
    return str;
}

double Utils::getBestValue()
{
    return this->valueBest;
}

nc::NdArray<double> Utils::getDistMat()
{
    unsigned int size=nCities;
    auto distMat=nc::NdArray<double>(nc::Shape(size,size));
    for(auto i=0;i<size;i++){
         for(auto j=0;j<size;j++){
             auto len=Point::getLen(coordinates[i],coordinates[j]);
             //auto len=nc::norm()
             distMat.at(i,j)=len;
             distMat.at(j,i)=len;
             //std::cout<<len<<std::endl;
         }
    }
    return distMat;
}

void Utils::init()
{
    this->distMat=getDistMat();
    tourNow=nc::arange(nCities);
    valueNow=calcTourMile();
    tourBest=tourNow.copy();
    valueBest=valueNow;
    recordNow=vector<double>();
    recordNow=vector<double>();
}

double Utils::calcTourMile()
{
    double mile=distMat.at(tourNow[nCities-1],tourNow[0]);
    for(int i=0;i<nCities-1;i++){
        mile+=distMat.at(tourNow[i],tourNow[i+1]);
    }
    return mile;
}

void Utils::mutateInvE(nc::NdArray<int> &tourInv, double &dEInv)
{
    auto n=nCities;
    int i=nc::random::randInt(1,nCities-1);
    int j;
    while(1){
        j=nc::random::randInt(1,nCities-1);
        if(i!=j) break;
    }
    if(i>j) swap(i,j);
    tourInv=tourNow.copy();
    for(int v=i;v<j+1;v++){
        tourInv[v]=tourNow[j+i-v];
    }
    dEInv=0.0;
    if(i==0){
        dEInv+=distMat.at(tourNow[n-1],tourNow[j])-distMat.at(tourNow[n-1],tourNow[i]);
    }else {
        dEInv+=distMat.at(tourNow[i-1],tourNow[j])-distMat.at(tourNow[i-1],tourNow[i]);
    }
    if(j==n-1){
        dEInv+=distMat.at(tourNow[0],tourNow[i])-distMat.at(tourNow[0],tourNow[j]);
    }else{
        dEInv+=distMat.at(tourNow[j+1],tourNow[i])-distMat.at(tourNow[j+1],tourNow[j]);
    }
}

void Utils::mutateSwapE(nc::NdArray<int> &tourSwap, double &dESwap)
{
    int i=nc::random::randInt(1,nCities-1);
    int j;
    while(1){
        j=nc::random::randInt(1,nCities-1);
        if(i!=j) break;
    }
    if(i>j) swap(i,j);
    tourSwap=tourNow.copy();
    swap(tourSwap[i],tourSwap[j]);
    dESwap = distMat.at(tourNow[i - 1], tourNow[j]) - distMat.at(tourNow[i - 1], tourNow[i])
                 + distMat.at(tourNow[i + 1], tourNow[j]) - distMat.at(tourNow[i + 1], tourNow[i])
                 + distMat.at(tourNow[j - 1], tourNow[i]) - distMat.at(tourNow[j - 1], tourNow[j])
                 + distMat.at(tourNow[j + 1], tourNow[i]) - distMat.at(tourNow[j + 1], tourNow[j]);
    if (i + 1 == j){
        dESwap = distMat.at(tourNow[i - 1], tourNow[j]) - distMat.at(tourNow[i - 1], tourNow[i])
                 + distMat.at(tourNow[j + 1], tourNow[i]) - distMat.at(tourNow[j + 1], tourNow[j]);
    }
}

void Utils::mutateMoveE(nc::NdArray<int> &tourMove, double &dEMove)
{
    int i=nc::random::randInt(1,nCities-1);
    int j,k;
    while(1){
        j=nc::random::randInt(1,nCities-1);
        k=nc::random::randInt(1,nCities-1);
        if(i!=j&&i!=k&&j!=k) break;
    }
    if(i>j) swap(i,j);
    if(i>k) swap(i,k);
    if(j>k) swap(j,k);
    auto frag1=tourNow[nc::Slice(0,i)];
    auto frag2=tourNow[nc::Slice(i,j)];
    auto frag3=tourNow[nc::Slice(j,k+1)];
    auto frag4=tourNow[nc::Slice(k+1,nCities)];
    tourMove=nc::concatenate({frag1,frag3,frag2,frag4});
    dEMove = distMat.at(tourNow[i - 1], tourNow[j]) - distMat.at(tourNow[i - 1], tourNow[i])
                 + distMat.at(tourNow[i], tourNow[k]) - distMat.at(tourNow[j - 1], tourNow[j])
                 + distMat.at(tourNow[k + 1], tourNow[j - 1]) - distMat.at(tourNow[k + 1], tourNow[k]);
}

double Utils::calcTourMile(nc::NdArray<int> tour,int n)
{
    double mile=distMat.at(tour[n-1],tour[0]);
    for(int i=0;i<n-1;i++){
        mile+=distMat.at(tour[i],tour[i+1]);
    }
    return mile;
}

